//
// Created by jg on 2021/1/6.
//

#ifndef POS_GENERATOR_GAZEBO_TO_POS_H
#define POS_GENERATOR_GAZEBO_TO_POS_H

#include <eigen3/Eigen/Dense>
#include <mutex>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>

#include <ros/ros.h>
#include <gazebo_msgs/GetModelState.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include "pos_generator/oriTrans.hpp"

class GAZEBO_POS{
protected:
    bool has_pos_;
    double robot_yaw_;

    nav_msgs::Odometry state_;
    std::mutex state_lock_;

    ros::ServiceClient client_;
    gazebo_msgs::GetModelState objstate_;
public:
    GAZEBO_POS() : has_pos_(false), robot_yaw_(0.0){};

    ~GAZEBO_POS(){}

    void init(ros::NodeHandle& nh);

    bool getPosition(nav_msgs::Odometry& state);

    bool getCloudPoint(const sensor_msgs::LaserScan &msg,
                       sensor_msgs::PointCloud2& obj, sensor_msgs::PointCloud2& ground);
};

#endif //POS_GENERATOR_GAZEBO_TO_POS_H
